#include <omplapp/config.h>
#include <ompl/tools/benchmark/Benchmark.h>
#include <omplapp/geometry/GeometrySpecification.h>
#include "RRTIterationCount.h"
#include "RRTConnectIterationCount.h"
#include "LazyRRTIterationCount.h"
#include "pRRTIterationCount.h"
//#include "PoissonDiskRRT.h"
//#include "PrecomputedPoissonDiskSampler.h"
#include "FCLStateValidityCheckerCounter.h"
#include "SE2RigidBodyPlanningValidityCheckCount.h"
#include <ompl/base/samplers/UniformValidStateSampler.h>
#include <boost/lexical_cast.hpp>
#include <boost/math/constants/constants.hpp>
#include <stdio.h>
#include "PoissonRRT.h"
#include "PoissonSampler.h"

using namespace ompl;

base::ValidStateSamplerPtr allocUniformStateSampler(const base::SpaceInformation *si)
{
	return base::ValidStateSamplerPtr(new base::UniformValidStateSampler(si));
}

void benchmark0(std::string& benchmark_name, SE2RigidBodyPlanningValidityCheckCount& setup,
	double& runtime_limit, double& memory_limit, int& run_count)
{
	benchmark_name = std::string("random_polygon");
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/RandomPolygons_planar_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting & goal state
	base::ScopedState<base::SE2StateSpace> start(setup.getSpaceInformation());
	base::ScopedState<base::SE2StateSpace> goal(start);

	// random polygons
	start->setX(-32.99);
	start->setY(42.85);
	start->setYaw(0.0);
	goal->setX(14.01);
	goal->setY(-43.15);
	goal->setYaw(0.0);

	setup.setStartAndGoalStates(start, goal);
	setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.setup();


	std::vector<double> cs(2);
	cs[0] = 35; cs[1] = 35; //cs[2] = 35;
	setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);


	runtime_limit = 3000.0;
	memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
	run_count = 100;
}

void benchmark1(std::string& benchmark_name, SE2RigidBodyPlanningValidityCheckCount& setup,
	double& runtime_limit, double& memory_limit, int& run_count)
{
	benchmark_name = std::string("maze");
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/Maze_planar_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting & goal state
	base::ScopedState<base::SE2StateSpace> start(setup.getSpaceInformation());
	base::ScopedState<base::SE2StateSpace> goal(start);

	// maze
	start->setX(0.01);
	start->setY(-0.15);
	start->setYaw(0);
	goal->setX(41.01);
	goal->setY(-0.15);
	goal->setYaw(46.0 * (double)boost::math::constants::pi<double>() / 180.0);

	setup.setStartAndGoalStates(start, goal);
	setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.setup();


	std::vector<double> cs(2);
	cs[0] = 35; cs[1] = 35; //cs[2] = 35;
	setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);


	runtime_limit = 3000.0;
	memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
	run_count = 1;
}

void benchmark2(std::string& benchmark_name, SE2RigidBodyPlanningValidityCheckCount& setup,
	double& runtime_limit, double& memory_limit, int& run_count)
{
	benchmark_name = std::string("unique_maze");
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/UniqueSolutionMaze_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/UniqueSolutionMaze_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting & goal state
	base::ScopedState<base::SE2StateSpace> start(setup.getSpaceInformation());
	base::ScopedState<base::SE2StateSpace> goal(start);

	// maze
	start->setX(-43.95);
	start->setY(-42.75);
	start->setYaw(0);
	goal->setX(44.05);
	goal->setY(45.25);
	goal->setYaw(0);

	setup.setStartAndGoalStates(start, goal);
	setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.setup();


	std::vector<double> cs(2);
	cs[0] = 35; cs[1] = 35; //cs[2] = 35;
	setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);


	runtime_limit = 3000.0;
	memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
	run_count = 100;
}

void benchmark3(std::string& benchmark_name, SE2RigidBodyPlanningValidityCheckCount& setup,
	double& runtime_limit, double& memory_limit, int& run_count)
{
	benchmark_name = std::string("bug_trap");
	std::string robot_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/car1_planar_robot.dae";
	std::string env_fname = std::string(OMPLAPP_RESOURCE_DIR) + "/2D/BugTrap_planar_env.dae";
	setup.setRobotMesh(robot_fname.c_str());
	setup.setEnvironmentMesh(env_fname.c_str());

	// define starting & goal state
	base::ScopedState<base::SE2StateSpace> start(setup.getSpaceInformation());
	base::ScopedState<base::SE2StateSpace> goal(start);

	// maze
	start->setX(7.02);
	start->setY(-12);
	start->setYaw(0);
	goal->setX(-36.98);
	goal->setY(-10);
	goal->setYaw(2.25147473507);

	setup.setStartAndGoalStates(start, goal);
	setup.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
	setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
	setup.setup();


	std::vector<double> cs(2);
	cs[0] = 35; cs[1] = 35; //cs[2] = 35;
	setup.getStateSpace()->getDefaultProjection()->setCellSizes(cs);


	runtime_limit = 3000.0;
	memory_limit = 10000.0; // set high because memory usage is not always estimated correctly
	run_count = 100;
}

void preRunEvent(const base::PlannerPtr &planner)
{
}

void postRunEvent(const base::PlannerPtr &planner, tools::Benchmark::RunProperties &run)
{
	run["collision checks INTEGER"] = boost::lexical_cast<std::string>(
		dynamic_cast<IValidityCheckCount*>(planner->getSpaceInformation()->getStateValidityChecker().get())->GetNumValidityChecks());
	run["iterations INTEGER"] = boost::lexical_cast<std::string>(dynamic_cast<IIterationCount*>(planner.get())->GetNumIterations());
}

int main(int argc, char **argv)
{
	SE2RigidBodyPlanningValidityCheckCount setup;
	std::string benchmark_name;
	double runtime_limit, memory_limit;
	int run_count;

	int benchmark_id = 0;
	int sampler_id = 1;
	int num_threads = 1;

	bool parallelOnly = false;

	if (argc > 1)
	{
		benchmark_id = boost::lexical_cast<int>(argv[1]);
	}
	if (argc > 2)
	{
		sampler_id = boost::lexical_cast<int>(argv[2]);
	}
	if (argc > 3)
	{
		num_threads = boost::lexical_cast<int>(argv[3]);
	}
	if (argc > 4)
	{
		parallelOnly = true;
	}

	switch (benchmark_id)
	{
	case 1:
		benchmark1(benchmark_name, setup, runtime_limit, memory_limit, run_count);
		break;

	case 2:
		benchmark2(benchmark_name, setup, runtime_limit, memory_limit, run_count);
		break;
	case 3:
		benchmark3(benchmark_name, setup, runtime_limit, memory_limit, run_count);
		break;

	case 4:
		break;

	case 0:
	default:
		benchmark0(benchmark_name, setup, runtime_limit, memory_limit, run_count);
		break;
	}

	// create the benchmark object and add all the planners we'd like to run
	tools::Benchmark::Request request(runtime_limit, memory_limit, run_count);
	tools::Benchmark b(setup, benchmark_name);

	request.useThreads = false;

	// optionally set pre & pos run events
	b.setPreRunEvent(boost::bind(&preRunEvent, _1));
	b.setPostRunEvent(boost::bind(&postRunEvent, _1, _2));

	if (!parallelOnly)
	{
		if (sampler_id == 1)
		{
			b.addPlanner(base::PlannerPtr(new poisson::PoissonRRT(setup.getSpaceInformation())));
			setup.getSpaceInformation()->setValidStateSamplerAllocator(poisson::PoissonSampler::allocPoissonStateSampler);
			b.setExperimentName(benchmark_name + "_poisson_sampler");
		}
		else
		{
			b.addPlanner(base::PlannerPtr(new RRTIterationCount(setup.getSpaceInformation())));
			b.addPlanner(base::PlannerPtr(new RRTConnectIterationCount(setup.getSpaceInformation())));
			b.addPlanner(base::PlannerPtr(new LazyRRTIterationCount(setup.getSpaceInformation())));
			setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
			b.setExperimentName(benchmark_name + "_uniform_sampler");
		}
	}
	else
	{
		const int THREAD_COUNT[] = { 1, 2, 4, 8, 16, 24, 32 };
		if (sampler_id == 1)
		{

		}
		else
		{
			for (int i = 0; i < 7; ++i)
			{
				b.addPlanner(base::PlannerPtr(new pRRTIterationCount(setup.getSpaceInformation(), THREAD_COUNT[i])));
			}
			setup.getSpaceInformation()->setValidStateSamplerAllocator(&allocUniformStateSampler);
		}
		b.setExperimentName(benchmark_name + "_parallel");
	}

	b.benchmark(request);

	char fileName[80];
#ifdef WIN32
	sprintf_s(fileName, 80, "benchmark%d%d_%d%s.log", benchmark_id, sampler_id, num_threads, parallelOnly ? "p" : "");
#else
	snprintf(fileName, 80, "benchmark%d%d_%d%s.log", benchmark_id, sampler_id, num_threads, parallelOnly ? "p" : "");
#endif
	b.saveResultsToFile(fileName);

	return 0;
}
